#ifndef _BUOLA_CV_CORNERS_H_
#define _BUOLA_CV_CORNERS_H_

#include <buola/image/format.h>
#include <buola/mat.h>

namespace buola { namespace cvi {

class CCorner
{
public:
    CCorner(const CCorner &pRH)
        :   mPixPos(pRH.mPixPos)
        ,   mImgPos(pRH.mImgPos)
        ,   mRCPos(pRH.mRCPos)
        ,   mWorldPos(pRH.mWorldPos)
        ,   mVariance(pRH.mVariance)
        ,   mAverage(pRH.mAverage)
        ,   mPatch(nullptr)
    {

    }

    CCorner()
        :   mPatch(nullptr)
    {

    }

    ~CCorner()
    {
        free(mPatch);
    }

    CPoint_d mPixPos;         //position in pixels
    CPoint_d mImgPos;         //position using focal length
    CPoint_d mRCPos;          //radial corrected position
    mat::CVec3d mWorldPos;    //3D world position
    double mVariance;       //corner patch variance
    double mAverage;        //corner patch average
    float *mPatch;          //8x8 image patch
    float mCZ;              //estimated z value used in disp tracker
};

struct CCornerMatch
{
    int mL,mR;
    float mStrength;
    float mDiff;    //diff with respect to second best
};

class CCornerList
{
public:
    typedef CCorner tCorner;

    CCornerList();
    CCornerList(const std::vector<std::pair<CPoint_d,float>> &pIn);
    ~CCornerList();

    void Clear();
    void Init(const std::vector<std::pair<CPoint_d,float>> &pIn);

    void CreatePatches(const img::CView_gray8 &pView);

    int size()                  {   return mCorners.size(); }
    CCorner &operator[](int i)  {   return *mCorners[i];    }

    const CRect_d &GetBBox()      {   return mBBox;           }

private:
    std::vector<CCorner*> mCorners;
//    CSize_i mImgSize;
//    float mThreshold;
//    int mVerbose;
//    const int mHBlkSize;
//    const int mCorSize;
    bool mHavePatches;
    bool mHaveStats;
    CRect_d mBBox;
};

class CStereoCornerMatcher
{
public:
    CPoint_d GetMaxDist()     {   return CPoint_d(320,10);  }
};

template<typename tType>
class CFixedSizeGrid
{
public:
    CFixedSizeGrid(int pX,int pY,int pCapacity)
        :   mX(pX)
        ,   mY(pY)
        ,   mCapacity(pCapacity)
    {
        mCount=new int[pX*pY];
        mData=new tType[pX*pY*pCapacity];

        for(int i=0;i<pX*pY;i++)
            mCount[i]=0;
    }

    ~CFixedSizeGrid()
    {
        delete[] mCount;
        delete[] mData;
    }

    int GetCount(int pX,int pY) const
    {
        return mCount[pX+pY*mX];
    }

    void Add(int pX,int pY,const tType &pData)
    {
        int lCount=mCount[pX+pY*mX]++;
        operator()(pX,pY,lCount)=pData;
    }

    tType &operator()(int pX,int pY,int i)
    {
        return mData[(pX+pY*mX)*mCapacity+i];
    }

    const tType &operator()(int pX,int pY,int i) const
    {
        return mData[(pX+pY*mX)*mCapacity+i];
    }

    int mX,mY,mCapacity;
    int *mCount;
    tType *mData;
};

float cross_correlate_64(float *pLH,float *pRH);

namespace detail
{
    class CCornerGridCalc
    {
    public:
        CCornerGridCalc(const CRect_d &pBBox,const CSize_d &pCellSize,const CSize_i &pNCells)
            :   mBBox(pBBox)
            ,   mCellSize(pCellSize)
            ,   mNCells(pNCells)
        {}

        CPoint_i GetCell(const CPoint_d &pPoint)
        {
            CPoint_i lRet;
            lRet.x=clamp((int)((pPoint.x-mBBox.l)/mCellSize.x),0,mNCells.x-1);
            lRet.y=clamp((int)((pPoint.y-mBBox.t)/mCellSize.y),0,mNCells.y-1);
            return lRet;
        }

        CRect_d mBBox;
        CSize_d mCellSize;
        CSize_i mNCells;
    };
}

template<typename tCornerList,typename tMatcher>
void match_corners(tCornerList &pLeft,tCornerList &pRight,std::vector<CCornerMatch> &pResult,tMatcher pMatcher)
{
    pResult.clear();

    //maximum distance between matched corners
    CPoint_d lMaxDist=pMatcher.GetMaxDist();

    //bounding box of all corners
    CRect_d lBBox=bounding_box(pLeft.GetBBox(),pRight.GetBBox());

    //number of cells of the grid we use to distribute the corners
    int lXCells=clamp((int)ceil(lBBox.w()/(lMaxDist.x/2)),1,64);
    int lYCells=clamp((int)ceil(lBBox.h()/(lMaxDist.y/2)),1,64);
    if(lXCells==2) lXCells=1;
    if(lYCells==2) lYCells=1;

    //size of each cell
    CSize_d lCellSize(lBBox.w()/lXCells,lBBox.h()/lYCells);

    CFixedSizeGrid<int> lLGrid(lXCells,lYCells,pLeft.size());
    CFixedSizeGrid<int> lRGrid(lXCells,lYCells,pRight.size());

    detail::CCornerGridCalc lGridCalc(lBBox,lCellSize,CSize_i(lXCells,lYCells));

    for(int i=0;i<pLeft.size();i++)
    {
        CPoint_i lCell=lGridCalc.GetCell(pLeft[i].mPixPos);
        lLGrid.Add(lCell.x,lCell.y,i);
    }
    for(int i=0;i<pRight.size();i++)
    {
        CPoint_i lCell=lGridCalc.GetCell(pRight[i].mPixPos);
        lRGrid.Add(lCell.x,lCell.y,i);
    }

    int lLMatches[pLeft.size()];
    float lLStrength[pLeft.size()];
    float lLDiff[pLeft.size()];

    for(int i=0;i<pLeft.size();i++)
    {
        float lMax=0.0;
        float lDiff=0.0; //diff with respect to second best
        int lMaxIndex=-1;
        typename tCornerList::tCorner &lL=pLeft[i];
        CPoint_d lPos=lL.mPixPos;
        CPoint_i lMinCell=lGridCalc.GetCell(lPos-lMaxDist);
        CPoint_i lMaxCell=lGridCalc.GetCell(lPos+lMaxDist);
        for(int x=lMinCell.x;x<=lMaxCell.x;x++)
        {
            for(int y=lMinCell.y;y<=lMaxCell.y;y++)
            {
                for(int n=0;n<lRGrid.GetCount(x,y);n++)
                {
                    int lRI=lRGrid(x,y,n);
                    typename tCornerList::tCorner &lR=pRight[lRI];

                    if(std::abs(lL.mPixPos.x-lR.mPixPos.x)>lMaxDist.x) continue;
                    if(std::abs(lL.mPixPos.y-lR.mPixPos.y)>lMaxDist.y) continue;

                    float lCC=cross_correlate_64(lL.mPatch,lR.mPatch);
                    if(lCC>lMax)
                    {
                        lMaxIndex=lRI;
                        lDiff=lCC-lMax;
                        lMax=lCC;
                    }
                    else if(lCC>lMax-lDiff) //this is the second best
                    {
                        lDiff=lMax-lCC;
                    }
                }
            }
        }

        lLMatches[i]=lMaxIndex;
        lLStrength[i]=lMax/64.0f;
        lLDiff[i]=lDiff/64.0f;
    }

    int lRMatches[pRight.size()];
    float lRDiff[pRight.size()];

    for(int i=0;i<pRight.size();i++)
    {
        float lMax=0.0;
        float lDiff=0.0; //diff with respect to second best
        int lMaxIndex=-1;
        typename tCornerList::tCorner &lR=pRight[i];
        CPoint_d lPos=lR.mPixPos;
        CPoint_i lMinCell=lGridCalc.GetCell(lPos-lMaxDist);
        CPoint_i lMaxCell=lGridCalc.GetCell(lPos+lMaxDist);
        for(int x=lMinCell.x;x<=lMaxCell.x;x++)
        {
            for(int y=lMinCell.y;y<=lMaxCell.y;y++)
            {
                for(int n=0;n<lLGrid.GetCount(x,y);n++)
                {
                    int lLI=lLGrid(x,y,n);
                    typename tCornerList::tCorner &lL=pLeft[lLI];

                    if(std::abs(lL.mPixPos.x-lR.mPixPos.x)>lMaxDist.x) continue;
                    if(std::abs(lL.mPixPos.y-lR.mPixPos.y)>lMaxDist.y) continue;

                    float lCC=cross_correlate_64(lL.mPatch,lR.mPatch);
                    if(lCC>lMax)
                    {
                        lMaxIndex=lLI;
                        lDiff=lCC-lMax;
                        lMax=lCC;
                    }
                    else if(lCC>lMax-lDiff) //this is the second best
                    {
                        lDiff=lMax-lCC;
                    }
                }
            }
        }

        lRMatches[i]=lMaxIndex;
        lRDiff[i]=lDiff/64.0f;
    }

    for(int i=0;i<pLeft.size();++i)
    {
        if(lLMatches[i]<0) continue;

        if(lRMatches[lLMatches[i]]!=i)
            continue;

        CCornerMatch lMatch;
        lMatch.mL=i;
        lMatch.mR=lLMatches[i];
        lMatch.mStrength=lLStrength[i];
        lMatch.mDiff=std::min(lLDiff[i],lRDiff[lLMatches[i]]);

        if(lMatch.mDiff>0.05f&&lMatch.mStrength>0.85f)
            pResult.push_back(lMatch);
    }
}

/*namespace cvi*/ } /*namespace buola*/ }

#endif /* CORNERS_H_ */
